clc
clear all
%urdf模型导入
robot = importrobot("C:\Users\Admin\Desktop\biped_v2\xacro\leg_v2_serial_left.urdf");
% [conf,JointVel,JointAccel,t]=stepper([0.04 -1.57 -2;0.2 1.57 2],1,100)
robot.DataFormat='column';
robot.Gravity =[0 0 -9.81];%%

%%根据urdf模型确定刚体系统自由度
N_fixed = 0;
for i = 1:1:robot.NumBodies
    if( strcmp(robot.Bodies{1, i}.Joint.Type,'fixed') == true)
        N_fixed=N_fixed+1;
    end   
end
N_dof = robot.NumBodies - N_fixed;

%定义连杆的惯量 N_dof个自由度的连杆
I_c=zeros(3,3,N_dof);
I_o=zeros(3,3,N_dof);
d_oc=zeros(3,N_dof);
m=zeros(1,N_dof); %质量

%根据所导入的urdf模型读取各连杆的质量 惯量（相对于各刚体坐标系原点） 和质心位置向量（相对于各个刚体坐标系原点）
for i = 1:1:N_dof
    m(i) = robot.Bodies{1, i}.Mass;
    d_oc(:,i) = [-0.0568    0.0001   -0.0494]';
end
%连杆1 link_left_hip  
m(1)=0.6262;
I_c(:,:,1)=[776019E-09 -712E-09 128330E-09;
            -712E-09   627085E-09 585E-09;
            128330E-09   585E-09  528910E-09];
d_oc(:,1)=[-0.0568 0.0001 -0.0494]'; 
%连杆2 link_left_thigh_base
m(2)=1.8264;
I_c(:,:,2)=[4741313E-09 3743E-09    -157393E-09;
            3743E-09    2405029E-09 -29199E-09;
            -157393E-09 -29199E-09  4606284E-09];
d_oc(:,2)=[0.0535 0.0259 -0.0553]';
%连杆3 link_left_thigh
m(3)=0.448;
I_c(:,:,3)=[1146E-06 -34E-06  -83E-06;
            -34E-06  1126E-06 289E-06;
            -83E-06  289E-06  560E-06];
d_oc(:,3) = [0.006764 0.018274 -0.077578]';
%连杆4 link_left_calf
m(4)=0.245;
I_c(:,:,4)=[1396.76E-06 -0.25E-06    9.327E-06;
            -0.25E-06   1442.077E-06 -0.079E-06;
            9.327E-06   -0.079E-06   102.718E-06];
d_oc(:,4) = [0.019399 -0.000164 -0.12196]';     
%连杆5 link_left_foot
m(5)=0.131856;
I_c(:,:,5)=[53406E-09   0          -30266E-09;
            0           167145E-09 0.447E-09;
            -30266E-09  0.447E-09  137711E-09]; 
d_oc(:,5) = [0.025662 0 -0.042076]';   

for i=1:1:5
    I_o(:,:,i)=I_c(:,:,i)+m(i)*skew_matrix(d_oc(:,i))*skew_matrix(d_oc(:,i))';
end

%模型得到的动力学参数集合 param =[mi micx micy micz i_xx i_xy i_xz i_yy i_yz i_zz]
param_model_matrix=zeros(10,5);
for i=1:5
    param_model_matrix(1,i)=m(i);
    param_model_matrix(2:4,i)=m(i)*d_oc(:,i);
    param_model_matrix(5,i)=I_o(1,1,i);
    param_model_matrix(6,i)=I_o(1,2,i);
    param_model_matrix(7,i)=I_o(1,3,i);
    param_model_matrix(8,i)=I_o(2,2,i);
    param_model_matrix(9,i)=I_o(2,3,i);
    param_model_matrix(10,i)=I_o(3,3,i);
%%
%     param_model_matrix(5,i)=robot.Bodies{1,i+1}.Inertia(1);
%     param_model_matrix(6,i)=robot.Bodies{1,i+1}.Inertia(4);
%     param_model_matrix(7,i)=robot.Bodies{1,i+1}.Inertia(5);
%     param_model_matrix(8,i)=robot.Bodies{1,i+1}.Inertia(2);
%     param_model_matrix(9,i)=robot.Bodies{1,i+1}.Inertia(6);
%     param_model_matrix(10,i)=robot.Bodies{1,i+1}.Inertia(3);
end
Para_vec=zeros(50,1);
Para_vec =[param_model_matrix(:,1);
           param_model_matrix(:,2);
           param_model_matrix(:,3);
           param_model_matrix(:,4);
           param_model_matrix(:,5)];

tau=zeros(5,1000);
tau_ideal=zeros(5,1000);
for i=1:1:1000
    for j = 1:5
        q(j,i) = pi/6*sin(2*pi*i/1000);
        dq(j,i) =pi/3*pi*cos(2*pi*i/1000);
%         dq(j,i) =pi*pi*cos(2*pi*i/1000);
        ddq(j,i) =-2*pi/3*pi*pi*sin(2*pi*i/1000);
%         q(j,i) = 0;
%         dq(j,i) =0;
        ddq(j,i) =0;
    end
    [U,Uj] = Compute_LinearDynmatrix(q(:,i), dq(:,i), ddq(:,i));
    tau_temp= Uj *Para_vec;
    tau_ideal_temp = inverseDynamics(robot,q(:,i),dq(:,i),ddq(:,i));
    tau(:,i)=tau_temp;
    tau_ideal(:,i)=tau_ideal_temp;
    if i == 200
        c=1;
    end
end

figure(1)
for j=1:5
    subplot(2,3,j)
    t = 0.001:0.001:1
    plot(t,tau(j,:),'-r')
    
    hold on
    plot(t,tau_ideal(j,:),'--g')
    
    xlabel('t(s)');
    ylabel('joint torque(Nm)')
end